A six axes joint type robot including three basic axes and three wrist axes is structured by the following six axes as shown in FIG. 6:
(1) A 1st axis 2 provided at the upper part of a base 1 fixed on the ground to have the degree of freedom around the perpendicular (Z.sub.0 axis) PA0 (2) A 2nd axis 3 provided at the end part of the 1st axis 2 to have the degree of freedom around the Z.sub.1 axis perpendicular to the Z.sub.0 axis PA0 (3) A 3rd axis 4 provided at the end part of the 2nd axis 3 to have the degree of freedom around the Z.sub.2 axis parallel to the Z.sub.1 axis PA0 (4) A 4th axis 5 provided at the end part of the 3rd axis 4 to have the degree of freedom around the Z.sub.3 axis perpendicular to the Z.sub.2 axis PA0 (5) A 5th axis 6 provided at the end part of the 4th axis 5 to have the degree of freedom around the Z.sub.4 axis perpendicular to the Z.sub.3 axis PA0 (6) A 6th axis 7 provided at the end part of the 5th axis 6 to have the degree of freedom around the Z.sub.5 axis perpendicular to the Z.sub.4 axis
The 1st axis 2 to 3rd axis 4 are called the three basic axes, while the 4th axis 5 to 6th axis 7 are called the three wrist axes.
As described above, each joint has one degree of freedom and the robot as a whole has six degrees of freedom.
In such a six joint type robot, each axis has a restricted operating range within .+-.180 degrees from the structural reason thereof.
Recently, there has been a demand for the expansion of the operating range of robots. For this purpose, the structure of robots has been modified to provide the axes for realizing rotation of .+-.180 degrees or more. Thereby, the operating range can be expanded up to the range under .+-.360 degrees with improvement of the control method.
Such improved control method will be explained in the sequence of the flowchart shown in FIG. 4 with a calculation example of link angle (angle of each axis in the joint coordinates system) of the wrist joint (the 6th axis 7 in FIG. 3).
In the case of realizing linear interpolating operation by the 6 axes joint type robot shown in FIG. 3, the robot is operated by converting the rectangular coordinate data (position data and posture data) into each joint coordinate data (reverse conversion) in every one unit clock (one sampling period) to obtain a command value of each axis in the joint coordinate system and by giving such command values to each drive axis.
An example of such reverse conversion is described in the Japanese Laid-open Patent No. 62-193786. Namely in the reverse conversion, the command values of link angles .theta..sub.1 -.theta..sub.6 are obtained by the first step for calculating the link angles .theta..sub.1, .theta..sub.2, .theta..sub.3 of three basic axes from the position data P.sub.x, P.sub.y, P.sub.z, the second step for calculating the link angles .theta..sub.4, .theta..sub.5, .theta..sub.6 of the wrist joints from the posture data A.sub.x, A.sub.y, A.sub.z (vectors of direction in which the robot hand comes near to an object), O.sub.x, O.sub.y, O.sub.Z (vectors for designating the direction of robot hand) and N.sub.x, N.sub.y, N.sub.z (vectors of tangent direction for designating three vectors to retake the right hand system) and the link angles .theta..sub.1, .theta..sub.2, .theta..sub.3 of three basic axes obtained in the first step, and the third step for calculating again the link angles of three basic axes from the above position data and the link angles obtained in the second step.
Step 40 in FIG. 4 indicates a step for obtaining the link angle of the 6th axis (link angle at the next position where the control point of the robot is to be moved to as a target) through the reverse conversion by the procedures explained above. In this case, the link angle of the obtained target value is defined as ANS. In step 40 of FIG. 4, x can be calculated from the formula EQU x=sin .theta..sub.6 /cos .theta..sub.6
which is prepared for obtaining ANS by the reverse conversion.
Here, the link angle of the target value ANS can be obtained from the position data and posture data, according to the reverse conversion method described above. This link angle is defined as a value uniquely within the range of -180.degree.-+180.degree. by obtaining a value of tan.sup.-1 x considering the signs of sin .theta..sub.6 and cos .theta..sub.6. However, the sign of angle which indicates the position is defined as + for clockwise from the reference position (0.degree.) and - for counterclockwise.
After obtaining the link angle of target value ANS, a difference between ANS and the link angle of current value LAST is obtained in step 41 and checked to determine whether the absolute value thereof ABS. (ANS-LAST) exceeds 180.degree. or not. This check is carried out to determine the moving direction so that the movement (rotating angle) is minimized.
Here, in case ABS (ANS-LAST).ltoreq.180.degree., correction of link angle of the target value is unnecessary. Therefore, the link angle of final target value ANS' is set equal to ANS (ANS'=ANS) in step 42.
In case ABS (ANS-LAST)&gt;180.degree., check for the link angle of target value ANS (ANS&gt;0.degree.) is carried out in step 43.
Upon reception of this result, correction of the link angle of target value ANS is carried out in steps 44 and 45. In this case, the link angle of target value ANS is corrected to ANS'=ANS-360.degree. in step 44 or to ANS'=ANS+360.degree. in step 45.
In this method, there are teaching points (S, E) as shown in FIG. 5A and it is here supposed to conduct the linear interpolating operation between two points. When the target point after the one unit clock from the point (P) in the interpolating operation is defined as P', position and posture data are expressed respectively as follows. ##EQU1##
The link angle of the 6th axis at the point P' can be calculated by the reverse conversion from the position and posture data.
In this case, when the current position of the 6th axis at the point P is, for example, +178.degree. and the calculation result at the point P' is -178.degree. (FIG. 5B), the operation command of the 6th axis is defined by the difference -178.degree.-178.degree.=-358.degree.. Such operation command which is excessive as the interpolating operation per unit clock causes excessive operation on the 6th axis. If the 6th axis cannot follow the operation command even with the maximum operation speed of the 6th axis, an alarm function operates as the protection function, resulting in disabling operations. If the target value ANS exceeds .+-.180.degree., such conditions are produced.
According to the method shown in FIG. 4, in the case of the above example, after -178.degree. is obtained as the target value ANS of the calculation result of the 6th axis at point P' in step 40, the absolute value of deviation between the current position of .+-.178.degree. and the next target position -178.degree. is obtained in step 41.
Namely, the absolute value of deviation of angle is expressed as follows. EQU ABS (ANS-LAST)=ABS (-178.degree.-178.degree.)=356.degree.&gt;180.degree.
Here, since ANS&lt;0.degree., ANS'=-178.degree.+360.degree.=182.degree. can be obtained in step 45. Therefore, the operation command of 6th axis becomes 182.degree.-178.degree.=+4.degree..
Thereby, the robot operates clockwise toward the target of corrected target value ANS'=+182.degree. (same as -178.degree. of ANS as the target position) from the current position LAST. This control system expands the easily operable range of robot to the angle under +360.degree. exceeding +180.degree..
However, it has become apparent that a problem occurs in the operation in which some axis needs to be rotated for 360.degree. or more and then is interpolated continuously.
Namely, in case there are two teaching points (S, E) as shown in FIG. 6A and the linear interpolating operation is carried out between these two points, it is supposed that the 6th axis rotates once or more at the interpolation starting point S and its current position is defined as +538.degree.. In this case, as a result of reverse conversion at the target position P' after one unit clock, when the 6th axis is calculated to -178.degree., the following decision is made in step 41 by the correcting process of FIG. 4 for the link angle of target value ANS=178.degree.. EQU ABS (ANS-LAST)=ABS (-178.degree.-538.degree.)=716.degree.
In this case, the processing of steps 43 and 45 are applied and the following result can be obtained. EQU ANS'=-178.degree.+360.degree.=+182.degree.
Here, when operation is carried out toward ANS from LAST, the operation of +182.degree.-538.degree.=-356.degree. must be conducted.
However, the ideal target operation after the one unit clock is the operation for +4.degree. and the operation data of -356.degree. is actually given to the robot. The operation commands which define excessive interpolating operations per one unit clock result in excessive operations for the 6th axis, which cannot be followed even by the maximum operation speed of the 6th axis. In this case, the alarm function as the protection function operates, disabling operations.
Above explanation has been made for the 6th axis 7 shown in FIG. 3 and this is also applied to all robot axes having the operation range of .+-.360.degree..
In the case where the robot axes having the operation ranges of .+-.360.degree. or more carry out multi-rotating (rotation more than .+-.360.degree.), the correct interpolating operation cannot be realized with the conventional method which can be used only for the operation of under .+-.360.degree..
It is therefore and object of the present invention to solve such problems and realize interpolating operation by robot when the operating range is expanded to .+-.360.degree. or more.